perm filename ARITH.SAI[AL,HE]2 blob
sn#295841 filedate 1977-07-27 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00009 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 ENTRY
C00006 00003 ! new_sval,new_v3ect,new_trans,new_frame
C00008 00004 ! v3dot, v3dist, v3angle, grfmul
C00011 00005 ! v3ect-valued procedures
C00018 00006 ! rotn valued procedures
C00024 00007 ! rotn extraction procedures
C00031 00008 ! trans & frame valued procedures
C00033 00009 ! v3cmp, rotcmp, & transcmp
C00035 ENDMK
C⊗;
ENTRY;
BEGIN "ARITH"
IFCR ¬DECLARATION(CREFFING) THENC DEFINE CREFFING=FALSE;ENDC
IFCR ¬CREFFING THENC
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "MACROS.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
EXTERNAL INTEGER PROCEDURE NEWARY(INTEGER LO, HI, DIMS);
ENDC
REDEFINE ARITHTERNAL = "INTERNAL";
REQUIRE "ARITH.HDR[AL,HE]" SOURCE_FILE;
! some initialization & "global" constants;
PROCEDURE IACNSTS;
BEGIN
RPTR(TINFO) Q;
! INITIALIZE ARITHMETIC CONSTANTS;
PROCEDURE MAKE_CONST (STRING ID; RPTR(VALU$) V);
BEGIN ! Coded by RF;
RPTR(VALU$) ITEMVAR IV;
IV ← NEW(V);
NEW_PNAME(IV,ID);
END;
TRUEV←NEW_SVAL(1);
FALSEV←NEW_SVAL(0);
RADIANS←NEW_SVAL(180./π);
PI←NEW_SVAL(π);
CM←NEW_SVAL(1./2.54);
GM←NEW_SVAL(16./454.);
LBS←NEW_SVAL(16.);
NILVECT←NEW_V3ECT(0,0,0);
XHAT←NEW_V3ECT(1,0,0);
YHAT←NEW_V3ECT(0,1,0);
ZHAT←NEW_V3ECT(0,0,1);
NEGXHAT ← NEW_V3ECT(-1,0,0);
NEGYHAT ← NEW_V3ECT(0,-1,0);
NEGZHAT ← NEW_V3ECT(0,0,-1);
NILROTN←AXW_ROTN(ZHAT,0);
NILTRANS←NEW_TRANS(NILROTN,NILVECT);
NILDEPROACH ← STATION ← NEW_FRAME(NILTRANS);
ZFLIPR←VTOV_R(ZHAT,NEGZHAT);
ZFLIPT←NEW_TRANS(ZFLIPR,NILVECT);
YPARK ← NEW_FRAME( NEW_TRANS(AXW_ROTN(YHAT,180),NEW_V3ECT(40,14,9) ) );
BPARK ← NEW_FRAME( NEW_TRANS(AXW_ROTN(YHAT,180),
NEW_V3ECT(43.53125,56.855,10.95875)) );
STAN_DEPROACH ← NEW_V3ECT(0,0,3);
MAKE_CONST("TRUE",TRUEV);
MAKE_CONST("FALSE",FALSEV);
MAKE_CONST("RADIANS",RADIANS);
MAKE_CONST("PI",PI);
MAKE_CONST("π",PI);
MAKE_CONST("CM",CM);
MAKE_CONST("GM",GM);
MAKE_CONST("INCH",TRUEV);
MAKE_CONST("INCHES",TRUEV);
MAKE_CONST("SEC",TRUEV);
MAKE_CONST("SECONDS",TRUEV);
MAKE_CONST("OZ",TRUEV);
MAKE_CONST("OUNCES",TRUEV);
MAKE_CONST("LBS",LBS);
MAKE_CONST("DEG",TRUEV);
MAKE_CONST("DEGREES",TRUEV);
MAKE_CONST("NILVECT",NILVECT);
MAKE_CONST("NILROTN",NILROTN);
MAKE_CONST("NILTRANS",NILTRANS);
MAKE_CONST("STATION",STATION);
MAKE_CONST("XHAT",XHAT);
MAKE_CONST("YHAT",YHAT);
MAKE_CONST("ZHAT",ZHAT);
MAKE_CONST("YPARK",YPARK);
MAKE_CONST("BPARK",BPARK);
MAKE_CONST("NILDEPROACH",NILDEPROACH);
END;
REQUIRE IACNSTS INITIALIZATION [2];
INTEGER SIMPROC SIGNUM(REAL V);
START_CODE
LABEL XIT;
SKIPN 1,V;
JRST XIT;
MOVNI 2,1;
CAIL 1,0;
MOVEI 2,1;
MOVE 1,2;
XIT: END;
! new_sval,new_v3ect,new_trans,new_frame;
! these routines create new records of the indicated types
and store away their parameters into the appropriate subfields;
INTERNAL RPTR(SVAL) PROCEDURE NEW_SVAL(REAL VAL);
BEGIN
RPTR(SVAL) S;
S←NEW_RECORD(SVAL);
SVAL:VAL[S]←VAL;
RETURN(S);
END;
INTERNAL RPTR(V3ECT) PROCEDURE NEW_V3ECT(REAL X,Y,Z);
BEGIN
RPTR(V3ECT) V;
V←NEW_RECORD(V3ECT);
V3ECT:X[V]←X;
V3ECT:Y[V]←Y;
V3ECT:Z[V]←Z;
RETURN(V);
END;
INTERNAL RPTR(TRANS) PROCEDURE NEW_TRANS(RPTR(ROTN) R;RPTR(V3ECT) P);
BEGIN
RPTR(TRANS) T;
T←NEW_RECORD(TRANS);
TRANS:R[T]←R;
TRANS:P[T]←P;
RETURN(T);
END;
INTERNAL RPTR(FRAME) PROCEDURE NEW_FRAME(RPTR(TRANS) T;
RPTR(TINFO) TINF(NULL_RECORD));
BEGIN
RPTR(FRAME) F;
F←NEW_RECORD(FRAME);
FRAME:VAL[F]←T;
FRAME:TINFO[F]←TINF;
RETURN(F);
END;
! given a trans or frame, this routine returns a trans;
INTERNAL RPTR(TRANS) PROCEDURE TRANS_COERCE(RPTR(TRANS,FRAME) TF);
BEGIN
INTEGER RT;
RT←RECTYPE(TF);
IF RT=LOC(FRAME) THEN
RETURN(FRAME:VAL[TF])
ELSE IF RT=LOC(TRANS)∨RT=0 THEN
RETURN(TF)
ELSE
RETURN(CHKREC(TF,LOC(TRANS))); ! an error *** ;
END;
! v3dot, v3dist, v3angle, grfmul;
! Note: a number of these routines employ an optional parameter
to allow the user to specify the vector record to hold
the return value. If this parameter is omitted or null,
then a new record will be allocated to hold the result;
INTERNAL REAL SIMPLE PROCEDURE V3DOT(RPTR(V3ECT) V1,V2);
START_CODE
! returns x[v1]*x[v2]+y[v1]*y[v2]+z[v1]*v[v2] ;
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 1,1(5);
FMPR 1,1(6);
MOVE 2,2(5);
FMPR 2,2(6);
MOVE 3,3(5);
FMPR 3,3(6);
FADR 1,2;
FADR 1,3;
END;
INTERNAL REAL SIMPLE PROCEDURE V3MAGN(RPTR(V3ECT) V);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P ='17;
SKIPN 5,V;
PUSHJ P,$RERR;
MOVE 1,1(5);
FMPR 1,1;
MOVE 2,2(5);
FMPR 2,2;
MOVE 3,3(5);
FMPR 3,3;
FADR 1,2;
FADR 1,3;
PUSH P,1;
PUSHJ P,SQRT;
END;
INTERNAL REAL SIMPLE PROCEDURE V3DIST(RPTR(V3ECT) V1,V2);
START_CODE
! Returns distance between v1 & v2;
EXTERNAL INTEGER $RERR;
DEFINE P ='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 1,1(5);
FSBR 1,1(6);
FMPR 1,1;
MOVE 2,2(5);
FSBR 2,2(6);
FMPR 2,2;
MOVE 3,3(5);
FSBR 3,3(6);
FMPR 3,3;
FADR 1,2;
FADR 1,3;
PUSH P,1;
PUSHJ P,SQRT;
END;
INTERNAL REAL PROCEDURE V3ANGLE(RPTR(V3ECT) V1,V2);
RETURN( ACOS( V3DOT(V1,V2)/(V3MAGN(V1)*V3MAGN(V2)) )*DEG );
INTERNAL REAL PROCEDURE GRFMUL(RPTR(V3ECT) G;RPTR(ROTN) R;RPTR(V3ECT) F);
RETURN(V3DOT(G,RVMUL(R,F)));
! v3ect-valued procedures;
INTERNAL RPTR(V3ECT) PROCEDURE SVMUL(REAL SF;RPTR(V3ECT) V1);
BEGIN
! scales vector v1 by real sf & returns the result;
RPTR(V3ECT) V;
DEFINE XX "<>" = <V3ECT:X>;
DEFINE YY "<>" = <V3ECT:Y>;
DEFINE ZZ "<>" = <V3ECT:Z>;
IF SF=0 THEN RETURN(NILVECT);
IF SF=1.0 THEN RETURN(V1);
V←NEW_RECORD(V3ECT);
XX[V]←SF*XX[V1];
YY[V]←SF*YY[V1];
ZZ[V]←SF*ZZ[V1];
RETURN(V);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3CROSS(RPTR(V3ECT) A,B);
BEGIN
! computes cross(a,b);
RPTR(V3ECT) AXB;
DEFINE XX "<>" = <V3ECT:X>;
DEFINE YY "<>" = <V3ECT:Y>;
DEFINE ZZ "<>" = <V3ECT:Z>;
AXB←NEW_RECORD(V3ECT);
XX[AXB]←YY[A]*ZZ[B]-ZZ[A]*YY[B];
YY[AXB]←ZZ[A]*XX[B]-XX[A]*ZZ[B];
ZZ[AXB]←XX[A]*YY[B]-YY[A]*XX[B];
RETURN(AXB);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3ADD(RPTR(V3ECT) V1,V2);
BEGIN
RPTR(V3ECT) V3;
IF V1=NILVECT THEN RETURN(V2);
IF V2=NILVECT THEN RETURN(V1);
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
MOVE 1,1(5);
FADR 1,1(6);
MOVEM 1,1(7);
MOVE 1,2(5);
FADR 1,2(6);
MOVEM 1,2(7);
MOVE 1,3(5);
FADR 1,3(6);
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3SUB(RPTR(V3ECT) V1,V2);
BEGIN
RPTR(V3ECT) V3;
IF V2=NILVECT THEN RETURN(V1);
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
MOVE 1,1(5);
FSBR 1,1(6);
MOVEM 1,1(7);
MOVE 1,2(5);
FSBR 1,2(6);
MOVEM 1,2(7);
MOVE 1,3(5);
FSBR 1,3(6);
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3LINC(REAL W1;RPTR(V3ECT) V1;
REAL W2;RPTR(V3ECT) V2);
BEGIN
! returns w1*v1+w2*v2;
RPTR(V3ECT) V3;
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
! x coord;
MOVE 1,1(5);
FMPR 1,W1;
MOVE 2,1(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,1(7);
! y coord;
MOVE 1,2(5);
FMPR 1,W1;
MOVE 2,2(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,2(7);
! z coord;
MOVE 1,3(5);
FMPR 1,W1;
MOVE 2,3(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3COPY(RPTR(V3ECT) V,NV(NULL_RECORD));
BEGIN
! returns a new vector with the same values as V;
IF NV=NULL_RECORD THEN
NV←NEW_RECORD(V3ECT);
V3ECT:X[NV]←V3ECT:X[V];
V3ECT:Y[NV]←V3ECT:Y[V];
V3ECT:Z[NV]←V3ECT:Z[V];
RETURN(NV);
END;
INTERNAL RPTR(V3ECT) PROCEDURE RVMUL(RPTR(ROTN) R;RPTR(V3ECT) V);
BEGIN
! produces a new vector with value R*V;
REAL X,Y,Z;
INTEGER I,II;
IF R=NILROTN ∨ V=NILVECT THEN RETURN(V);
II←MEM[LOC(V)]; X←Y←Z←0;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
X←X+MEM[II+I,REAL]*ROTN:RMX[R][I,1];
Y←Y+MEM[II+I,REAL]*ROTN:RMX[R][I,2];
Z←Z+MEM[II+I,REAL]*ROTN:RMX[R][I,3];
END;
RETURN(NEW_V3ECT(X,Y,Z));
END;
INTERNAL RPTR(V3ECT) PROCEDURE RIVMUL(RPTR(ROTN) R;RPTR(V3ECT) V);
BEGIN
! produces a new vector with value INV(R)*V;
REAL X,Y,Z;
INTEGER I,II;
IF R=NILROTN ∨ V=NILVECT THEN RETURN(V);
II←MEM[LOC(V)]; X←Y←Z←0;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
X←X+MEM[II+I,REAL]*ROTN:RMX[R][1,I];
Y←Y+MEM[II+I,REAL]*ROTN:RMX[R][2,I];
Z←Z+MEM[II+I,REAL]*ROTN:RMX[R][3,I];
END;
RETURN(NEW_V3ECT(X,Y,Z));
END;
INTERNAL RPTR(V3ECT) PROCEDURE TVMUL(RPTR(TRANS) T;RPTR(V3ECT) V);
RETURN(V3ADD(TRANS:P[T],RVMUL(TRANS:R[T],V)));
INTERNAL RPTR(V3ECT) PROCEDURE TIVMUL(RPTR(TRANS) T;RPTR(V3ECT) V);
RETURN(RIVMUL(TRANS:R[T],V3SUB(V,TRANS:P[T])));
INTERNAL RPTR(V3ECT) PROCEDURE UVECT(RPTR(V3ECT) V;REAL SLOPOK(0.0));
BEGIN
! this procedure transforms V into a unit vector & returns VHAT
IF abs(v.v-1.0)≤slopok then assumes that vector is already unit.
;
REAL M;
M←V3DOT(V,V);
IF ABS(M-1.0)≤SLOPOK THEN
RETURN(V)
ELSE IF M=0 THEN
BEGIN
USERERR(1,1,"UVECT: 0 VECTOR");
RETURN(V);
END
ELSE
RETURN(SVMUL(1/SQRT(M),V));
END;
! rotn valued procedures;
RPTR(ROTN) PROCEDURE AXW_R(RPTR(V3ECT) AX;
REAL W;RPTR(ROTN) R(NULL_RECORD));
! **** WARNING: Do NOT supply a rotation R
to this unless αL & βL are correct ****;
BEGIN
! produces a ROTN, given axis vector AX & magnitude W;
SAFE REAL ARRAY RMX[1:3,1:3];
REAL CX,CY,CZ,SW,CW;
AX←UVECT(AX);
CX←V3ECT:X[AX];
CY←V3ECT:Y[AX];
CZ←V3ECT:Z[AX];
IF R=NULL_RECORD THEN
BEGIN
R←NEW_RECORD(ROTN);
ROTN:αL[R]←ACOS(CZ)*DEG;
ROTN:βL[R]←ATAN2(CY,CX)*DEG;
END;
ROTN:MAGN[R]←W;
ROTN:AXIS[R]←AX;
SW←SIND(W);CW←COSD(W);
RMX[1,1]←CW+(1-CW)*CX↑2;
RMX[2,1]←(1-CW)*CX*CY-CZ*SW;
RMX[3,1]←(1-CW)*CX*CZ+CY*SW;
RMX[1,2]←(1-CW)*CX*CY+CZ*SW;
RMX[2,2]←CW+(1-CW)*CY↑2;
RMX[3,2]←(1-CW)*CY*CZ-CX*SW;
RMX[1,3]←(1-CW)*CX*CZ-CY*SW;
RMX[2,3]←(1-CW)*CY*CZ+CX*SW;
RMX[3,3]←CW+(1-CW)*CZ↑2;
MEM[LOC(ROTN:RMX[R])]←MEM[LOC(RMX)];
MEM[LOC(RMX)]←0;
RETURN(R);
END;
INTERNAL RPTR(ROTN) PROCEDURE AXW_ROTN(RPTR(V3ECT) AX;REAL W);
RETURN(AXW_R(AX,W));
INTERNAL RPTR(ROTN) PROCEDURE RINVRT(RPTR(ROTN) R);
BEGIN
! returns inverse rotation to R;
RPTR(ROTN) RINV;
IF R=NILROTN THEN RETURN(NILROTN);
BEGIN
SAFE REAL ARRAY RMX[1:3,1:3];
RINV←NEW_RECORD(ROTN);
ARRTRAN(RMX,ROTN:RMX[R]);
ROTN:AXIS[RINV]←ROTN:AXIS[R];
ROTN:αL[RINV]←ROTN:αL[R];
ROTN:βL[RINV]←ROTN:βL[R];
MEM[LOC(ROTN:RMX[RINV])]↔MEM[LOC(RMX)];
END;
ROTN:RMX[RINV][1,2]↔ROTN:RMX[RINV][2,1];
ROTN:RMX[RINV][1,3]↔ROTN:RMX[RINV][3,1];
ROTN:RMX[RINV][2,3]↔ROTN:RMX[RINV][3,2];
ROTN:MAGN[RINV]←-ROTN:MAGN[R];
RETURN(RINV);
END;
INTERNAL RPTR(ROTN) PROCEDURE αβW_ROTN(REAL αL,βL,W);
BEGIN
! builds a rotation of magn W about an axis specified by
angles αL & βL;
RPTR(ROTN) R;
R←NEW_RECORD(ROTN);
ROTN:αL[R]←αL;ROTN:βL[R]←βL;
RETURN(
AXW_R( NEW_V3ECT(SIND(αL)*COSD(βL),SIND(αL)*SIND(βL),COSD(αL)),
W,
R));
END;
INTERNAL RPTR(ROTN) PROCEDURE RRMUL(RPTR(ROTN) R1,R2);
BEGIN
! produces a new rotation equiv to R1*R2;
SAFE OWN REAL ARRAY RMX[1:3,1:3];
INTEGER I,J,K,L;
DEFINE XX(I) "<>" = <RMX[I,1]>;
DEFINE YY(I) "<>" = <RMX[I,2]>;
DEFINE ZZ(I) "<>" = <RMX[I,3]>;
! ugh! blech! column order sure is confusing when you
have been using row order for the past eight years! ;
ARRCLR(RMX);
FOR I←1 STEP 1 UNTIL 3 DO
FOR J←1 STEP 1 UNTIL 2 DO
FOR K←1 STEP 1 UNTIL 3 DO
RMX[J,I]←RMX[J,I]+ROTN:RMX[R1][K,I]*ROTN:RMX[R2][J,K];
XX(3)←YY(1)*ZZ(2)-ZZ(1)*YY(2);
YY(3)←ZZ(1)*XX(2)-XX(1)*ZZ(2);
ZZ(3)←XX(1)*YY(2)-YY(1)*XX(2);
RETURN(RMX_ROTN(RMX));
END;
INTERNAL RPTR(ROTN) PROCEDURE VTOV_R(RPTR(V3ECT) V1,V2;REAL SLOPOK(0.0));
BEGIN
! returns a rotation that will make V1 lie parallel to
V2. If the cosine of the angle between V1 & V2 is
less than SLOPOK, then NILROTN will be returned.
;
RPTR(V3ECT) V3;
REAL C,W0;
V1←UVECT(V1);V2←UVECT(V2);
C←V3DOT(V1,V2);
IF ABS(1.0-C)≤SLOPOK THEN
RETURN(NILROTN); ! treat these vectors as parallel;
IF ABS(1.0+C)≤SLOPOK THEN
BEGIN ! they are 180 degrees apart;
IF ABS(V3ECT:X[V1])<.6 THEN V3←XHAT
ELSE IF ABS(V3ECT:Y[V1])<.6 THEN V3←YHAT
ELSE IF ABS(V3ECT:Z[V1])<.6 THEN V3←ZHAT;
V3←V3CROSS(V1,V3);
W0←180;
END
EHSE
BEGIN
V3←V3CROSS(V1,V2);
W0←ATAN2(V3MAGN(V3),C)*DEG;
END;
RETURN(AXW_ROTN(V3,W0));
END;
INTERNAL RPTR(ROTN) PROCEDURE VVROT(BPTR(V3ECT) X,Z);
BEGIN
! retqrns a rotatiOn that will pu@PA5⊃βPAaCe¬YYKX↓i↑~∀$@A4@_AoQS
PAoS1XAakβ!αb"
!β';&yβS#*αbiβ∧cπ;∃qX4(∀PJJBR∩BJ>Rp¬∩¬∪1Q M∃zjD⎇1
%¬DD~AEBKαc"A≠↔uLiI3Pjε∃L ⊗¬E5Lq z
⊗β ∀Wk→Q'j∀- -∀V-
]FE∧T"j*i∪∀))&Uf∀+*∪k )∀∀+&jf
)⊗,$⊂j⊂ ,X)(R));
END;
! rotj e@aieCGβ#'?9πβK/∂.#WK↔≠X4(Q*4Lm HR¬∀X→B¬¬)x4,%Z(R¬≥8∧4JE∀Q0)D∃J.aQB4Q*J4SJ _H ,¬@____ _↑+≡ ⊂*$"Sα 0.0 ELSE SQRT(V));
~∃M∪≠!→∀A% β0A!%∨
+∀)αεε≤zM"J,
1α
KX4(&∀*RVJrB&→↓i 0∪βεεββ β⊂o¬V+Lλ
I⊃3@@PεE∧H⊂⊂⊂⊂λ⊂"f)QP F 1*0000<C<1,∧```@`bA$B⊗)↓αq@4!∀ααα∧∧α∧,J8R∧ 9z2D~∃↔0hPβ"R)j⊃0 '⊂f⊂ ∩PTR(V3ACT) PROCEDURE AXIS(RPTR(ROTN$A$Rv4∀∪¬ ≥∪≤4Ph(%¬∧∧WGπ,≤7'~∞Mε*ε∨
↔4≠yH∞-βz0z~ww⊂ &pom a ROTN;
! Wri@QiK\A rAβ%≤@lZnXv~∀~(∪% β0Aπ0Y
2Yπ41αY∧Y Yπ.v4∀~∧∪¬?%∨)8p
J6β 5∃m6⊃C
kαc"A_WtSjINTS+ tW6f% W ≥CE C←ROTN~RMX[R][3,3];
CW←(A+B+C-1)/2;
∪∪_Aπ.@x`\rrdrA)⊃∃≤~∀∩%% )+I≤Q5⊃¬(R∩Bαβ[↔∂&{Iβ≠xε"∧t→J$⎇#1Q L,J8PhP⊃_$,<α3C!!"4Q(→λ⊃εaQB"1 vk0+(%0nc!! 0vKztt4JE
0%XJpjf∃+q
'1"B"(≠(↔h _H⊂vGF L ε∀∃∩⊃)dλ∀tj~U
¬X*pK(5l*+hE#"B!⊃13∀hTλ
∀Iz∪NTI[⊗p .VY⊗→nCE∧DDBVP!'U'≥)&V-i.mL⊗_nE∀'j'≥∀&l-i↔mXV→WWT VLTTU!V⊂εE∧BDDWP
_Va∃T'j'≥∀&l-i↔mY⊗_WU)'j∪≥)&l⊗i.mXK→.WT⊂VXTTNFE∧DPl⊂/P∩c⊂!m
`a)T⊂βY)<0,001THEN SSQRT((A-B-C+!)/D)
ELSE -(ROTN:RMX[R][1,2]*CY + ROTN:RMX[R][1,3]*CZ) / (A-1);
RETURN(NEW_V3ECT(CX,CY,CZ));
END
END;
INTERNAL RPTR(SRAL) PROCEDURE RMAGN(RPTR(ROTN) R ;
BEGAN
! finds the angle of rotation for a ROTN;
! Written by ARG 6-76;
RAAL CX,CY,CZ,A,B,C,CW;
RPTR(SVAL) S;
S←NEW_RACORD(SVAL);
A←ROTN:RMX[R][1,1];
B←ROTN:RMX[R][2,2];
C←ROTN:RMX[R][3,3];
CW←(A+B+C-1)/2;
IF CW >0.9999 THEN
SVAL:VAL[S]←0
ELSE
BEGIN
RAAL D;
D←3-A)B-C;
AZ←SSQRT((-A-B+C+1)/D)3
CY ← IF CZ<0.001 THEN SSQRT((-A+B-C+1)/D)
ELSE (ROTN8RMX[R][2,3]
-ROTN:RMX[R][2,1]*ROTN:RMX[R][1,3]/(A-1))*CZ
- (1-B+ROTN:RMX[R][2,1]*ROPN:RMX[@%u6bXetZQαZDRRf~(∩∪π0αα⎇α&2α∞i. ∩M"∞JIqA9β↓EαRD*9αN≥
JQ!D 6 6~YE%>"H4($HJ⊗2N*↓5"J⎇"9jJmBnJvY 1JuT~e↓-¬∩>R9U∩6bn∃jmE1≥i*∞iJ↓=↓"
iE%lhP$&N4
1jZ bnNv|
ε∞>~B∞]%T"⊗≥lhP$&&2αε
MD~i%j↓A9U;9αR",q4(HH&
⊗<J84(HH&&→αBJ>RsRJ6b]∩vmIc
u6J⎇"9jJmBnJv[ 1JuJz∞i↓r↓AαRD*84(HH$&N4
1jZ bnNvzjNJεcRZε2]~ul4PH$&⊗t 4($L*2N∃∧J→αε∃→"∞eIiA9U;9αR",p4($HJ
⊗≡Lp4($HJ&→↓E∩>R9U∩6bn∃jmE1≥i6J>$qjJ6EZJvm~aFu%|~e↓yβ↓αR",p4($HH&NZ ajZεeZNv⎇m~Zε1U2ε2n≥il4(HH&⊗: h($&,bN∃αL1αε
~B∞a$k↓9U]:αR"⊗ph($$L∩⊗≡&ph($$LJ→↓"∀zR9j∀jbnJmYM1JjjJ>RsRJ6b]∩vmIc~u%>≥A↓y↓ααR"⊗ph($$HJNZεcRZε2]~v⎇6≥2ε1j4
2nNkX4($HJ⊗:⊂hP$&⊗e~∀4(HH&VN-∩⊗JIC 1E1∃∩6ε≡sQαNR∀
:≡⊗t*NM¬∩Il4(HJ⊗:⊃Xh(&J-"VJ9E→%l4PJ⊗:⊃Xh(4*LrR⊗Jt
1αJ¬"I"J⎇"9%α¬∩>∞⊗%*J∃α∀j`bJ⎇"9"J,
1αε∃∩εeα∀jba%Xh(&
,:&84Ph(%¬ε∪W'3'→β¬β⊗{SπSN{91β>K[↔9ε βK?& S'?rβ7πS⊗KaαJmBa84PI↓α∪}+Mβ;␈!βπ≠6+∂Qα∀jbalhP4(%
α7?∪N3'↔⊃π#=β←␈∪-βC⊗{C↔KgIβ e∧
J≥↓2i]YlhP4(&≤
~∃α∀*ε1α
∩Jεe¬∩6bm QM1ES~ul4PJJ⊗εbα∞a2≥I2∞id 2 2~b∞]lhP&JB%⊃"J>$q%αIXh(4(L
JJR∀
9"JmA2J6EA%l4Ph(&ε⎇∩6bm
aFun∃zJ6b[⊃1Ju\~}J6EYM1NkX4(&≥:⎇"¬\⊃.
5
I=IlhP&&→∧~]↓yαqeeeJαR"⊗ph($&∀*RVJrB:&2∀zR9$hP&⊗2≤(4($L∩⊗≡&ph($&∀*ε1α#X4($M∩}:⊗9BJ⊗∞⎇∩⊃"J⎇"9%lhP$&∩{→6¬6∩j
l4PH&∞j⎇~NFJ"A!6¬l⊃.
-
I>⊃%Xh($&≥Iα⎇αL1α∞ic↓9AAλαR"⊗r↓αNN
∩Q!!l . 6~YE%>"H4($HJ⊗2N*↓↓"JmBmI1≥i↓5α∀jbmIc
u*JmBmE1≥i="¬k %%*≥Q4(HH$%=αAE6 ]∩6bm∩aFu*∀jbmEc∩u="
iE%%Xh($&≥Aα⎇αL1α∞i\
M"≥I%qAs↓AEα$B⊗9α≥~FJQBB¬& l→-E%|!$4(HH&⊗2≤)↓5"∀jbmEc∩u*∞J↓-αJmBmE1≥i*∞iJ↓=↓"
iE%lhP$&J⎇"9jεDJNnJmz:⊗\E1N⊗∞ B∞aH;∩d≥%↔0hP⊃~$⎇$g)T <k:%m|__4⎇~λ:rJTHXsXh!⊃∀L2λ_%~D;%∩↓jε¬c+;t
DD,d↓PPH⊃_$,<α3C!!""2(d
∀S+ lK +U4S6 6+ W%∃pvHπd λ∃ λ3C"A⊃""4Iz∪NS(_sVtK[k4SjILS0(yVtW'1"B"!_3Qβ!!"13
8(∩1Dλ0Tjλ;*',¬f-m`⊂∃$"gεB∧DDa⊃c` N
IF (RMX[1,3]-RMX[3,1])/CY > 0 THEN
ROPN:MAGN[R]←-ROTN2MAGJ[@%tv~∀∩$∪ ≥λ4∀∩&,bN∃αL1αε
~B∞a∧k↓9U]8∧¬$DYaPPH⊃_$,<α3C!!""2(d
⊂S+ lk KU4S6 6K w%∃pvπd λ∃ λ3C"A⊃""4Iz∪JS(_sVtK[k4SjIL¬&`Qg-`∩];
END
↓ELSE¬
USERERR(1,1,"RMX_ROTN: SPRANGENESS!");
ROP ≤@P∩2nJmzεε∞⎇→"∞iJR∩ε≥Xh($&∀zR9h≤bnJv|
Rε9∩B∞e2≥A%*∩,9l4(HJ⊗:⊃Xh(&6,jn2≡~BJ>RsRJ6b]∩u&v|j⊗6@9It~E)[αMkαc"A→136iIpj∀I[
77fπc"B*(5∃4Ie∀J.aQB13HGc"C! 3U⊃*)P3λ
*∃∀J
)u∪B$
∀SphX∃4Q$ tR1)j
∀T
JJ⊃∀H→Tj(
E.c"A_Q1r)a"B4J
∀J∀Iz∪J(
'c"B*+u∀P)jnTVjK.c"A~Q5∃*)J∀S+β∀Su e∀Su gTS6 :W**'1"B1)h∞c"AQR3UλZSP3∧
T∃∀E
Lq0jE(∀∀Ixq1∃*((∀∪j5∀T∃
%∃∀P)jj(∃¬↔c"B((1r3AQB4T
JJ∃LhXu
(
e∃nc!!5wsHZf∀Q(9tQ
fq0u¬↔c"B*ku∀P)jnT⊗jK.c"A~Lq0jGV⊗uk[uLq(:∞V⊗jk.c"A~Lq0jGV6uk[uLq(:∞V6jk.c"A~Lq0jGVVuk[uLq(:∞VVjk.c"A~Q5∃*)J∃j'1"B1)h∞c"AQ@ ! trans & frame valued procedures;
INTERNAL RPTR(TRANS) PROCEDURE TTMUL(RPTR(TRANS) T1,T2);
BEGIN
! returns trans T1*T2;
RPTR(TRANS) T;
IF T1=NILTRANS THEN RETURN(T2);
IF T2=NILTRANS THEN RETURN(T1);
T←NEW_RECORD(TRANS);
TRANS:R[T]←RRMUL(TRANS:R[T1],TRANS:R[T2]);
TRANS:P[T]←V3ADD(TRANS:P[T1],RVMUL(TRANS:R[T1],TRANS:P[T2]));
RETURN(T);
END;
INTERNAL RPTR(TRANS) PROCEDURE TINVRT(RPTR(TRANS) T);
BEGIN
! returns inverse trans to T;
RPTR(TRANS) TINV;
IF T=NILTRANS THEN RETURN(NILTRANS);
TINV←NEW_RECORD(TRANS);
TRANS:R[TINV]←RINVRT(TRANS:R[T]);
TRANS:P[TINV]←SVMUL(-1.0,RVMUL(TRANS:R[TINV],TRANS:P[T]));
RETURN(TINV);
END;
INTERNAL RPTR(TRANS) PROCEDURE VVVTRANS(RPTR(V3ECT) P,X,Z);
RETURN(NEW_TRANS(VVROT(V3SUB(X,P),V3SUB(Z,P)),P));
! ie a trans that moves the origin to P, tilts the
z-axis to point along z-p & puts xhat into the
(z-p)(x-p) plane;
! v3cmp, rotcmp, & transcmp;
!Thes@∀AI↑A1KqSG¬XAG←5aCeSM←]fA=LAmK
i←efLAie¬]gM←IZA[CQeSGKL\~∀∩4b@\\8ACeND@xACINd
∀$@`@\8\ACe≤b@zA¬eNd~(∩Vb@8\\ACINb@|αβπK≥⊂h)↓↓Xh(4
LrR⊗Jt
1α&u"⊗≡⊗∩αBJ>≤*∩VJ*αQN∞m↓"JB%⊃"YN,~Q%α3 2YIKX4(&≥"εJPD~>∩∀hP&2ε∀*1↓↓¬B&QIG0hP→Yu4,⊃∩∩cβ1Q LlzhPK∩Jf∪Xh!→T⎇4Q⊗2e3'1PPLYzd(KEK2Bk4 E≤Bε↔αJα4ε∃kXQ!∀E∀I⊃∪∩cG1PPLλ)DHK5FCXh)G LlzhPK*Hε#Xh!_4 lxQ∪*dε70hP~9tTλ⊗∃EDMG1PPL8→T(KUHβ≠XQ!∀ |αP"&∃⊗∩5π1"B0)XRSB&E∪∞c!+∩5∞A_3Q≥CEαE$S*"`∩NAL INTAGER PROCEDURE ROTCMP(@%A)$Q∀zR9%¬⊃E2I∩Il4(L∩⊗≡→aPPL→jD,<Z$∧KXQ!∀M⎇f84mα
)u$s(≠∧M≥:&∃je)zDsT≠ ∃≥]&+RKXQ!∃∀-JZ$rD_d∧J¬IλTr∧∀λTe≤T
4L<jYRE∀zIcTl_ye]∪≠UU∀⎇Ig$l yk5∪∃U∃∪Xh!_Tt#1Q hT→jD-∀h→B∧LjHT<-$
¬∀|8XE-∀T
E∀ j84mα
*¬%∩
J$ u5∀¬#
JF"KXQ!∀∀ y→`hP→→e$,xZ"∧K1Q LMzf4≤m¬
E∀ j7%¬]F≠Re%(→e≠U :C∃j↔1PPM(ZE-∀e ∀2∧∀
DD,d ∩∧,J8R¬∀zH4mα
J$ u7*%] 7 ⊗∃) g)N)-j→↔TT]FB∧bg"∞FEεE⊃g"⊂⊃⊂i$j$λ≥FE